/*
 * PWM_Motor32.c
 *
 * Created: 17-03-2013 01:37:04
 *  Author: Karmu
 */ 


#define F_CPU 8000000UL         /* 8 MHz crystal clock */
#include <avr/io.h>
#include <stdlib.h>
#include <string.h>
#include <util/delay.h>
#include <util/atomic.h>
#include <avr/interrupt.h>
#include "LCD.h"				/* Using Peter Fleury's LCD library */
#include "AVR035.h"

/* constant definitions */
static const PROGMEM unsigned char copyRightChar[] =
{
	0x07, 0x08, 0x13, 0x14, 0x14, 0x13, 0x08, 0x07,
	0x00, 0x10, 0x08, 0x08, 0x08, 0x08, 0x10, 0x00
};
static const PROGMEM unsigned char blockChar[] =
{
	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
	0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF
};

/* defines for motor control */
#define M_PORT PORTD
#define M_DDR DDRD

#define M_IN1 M_PORT, PD6
#define M_IN2 M_PORT, PD7
#define M_PWM1 M_PORT, PD5 //OC1A - LEFT

#define M_IN3 M_PORT, PD1
#define M_IN4 M_PORT, PD0
#define M_PWM2 M_PORT, PD4 //OC1B - RIGHT

#define M_LEFT		1
#define M_RIGHT		2

#define M_FORWARD	0
#define M_REVERSE	1
#define M_STOP		2
#define M_STALL		3

/* defines for sensors */
#define IR_DDR DDRB				/* DDRx - register for IR output */
#define IR_PORT PORTB			/* PORTx -register for IR output */
#define IR_PIN PINB				/* PINx - register for IR input  */
#define IR_BIT_LEFT PB0     /* bit for IR input/output */
#define IR_BIT_RIGHT PB1    /* bit for IR input/output */
#define IR_BIT_CENTER PB2   /* bit for IR input/output */
#define IR_BIT_REAR PB3     /* bit for IR input/output */

#define IR_LEFT IR_PIN, IR_BIT_LEFT       /* bit for IR input/output */
#define IR_RIGHT IR_PIN, IR_BIT_RIGHT     /* bit for IR input/output */
#define IR_CENTER IR_PIN, IR_BIT_CENTER   /* bit for IR input/output */
#define IR_REAR IR_PIN, IR_BIT_REAR       /* bit for IR input/output */

/* define SENSOR STATEs */
#define S_OFF						0
#define S_LEFT						1
#define S_RIGHT						2
#define S_CENTER					4
#define S_REAR						8
#define S_LEFT_RIGHT				3
#define S_LEFT_CENTER				5
#define S_CENTER_RIGHT				6
#define S_LEFT_CENTER_RIGHT			7
#define S_LEFT_REAR					9
#define S_RIGHT_REAR				10
#define S_LEFT_RIGHT_REAR			11
#define S_CENTER_REAR				12
#define S_LEFT_CENTER_REAR			13
#define S_CENTER_RIGHT_REAR			14
#define S_LEFT_CENTER_RIGHT_REAR	15

/* define BOT STATEs */
#define GO_LEFT 0
#define GO_HARD_LEFT 1
#define GO_FORWARD 2
#define GO_REVERSE 3
#define GO_HARD_RIGHT 4
#define GO_RIGHT 5
#define GO_BRAKE 6

/*define delays*/
#define DELAY 1000
#define LCDDELAY 200

#define REVERSE_DELAY 500
#define NEW_COURSE_DELAY 2000

/* define TOP & BOTTOM for PWM (ICR1) */
#define TOP			10000 //8 MHz 1 pre-scalar - decides frequency
#define BOTTOM		5000

/*Global variables*/
volatile uint32_t timer0_milliseconds = 0;

/* ISR */
ISR(TIMER0_OVF_vect) {
	timer0_milliseconds++;
}

void init(){
	/* enable interrupts */
	sei();
}

void io_init(void)
{
	/* setup input/outputs */
	M_DDR |= _BV(M_IN1) | _BV(M_IN2) | _BV(M_IN3) | _BV(M_IN4) | _BV(M_PWM1) | _BV(M_PWM2);
	
	IR_DDR &= ~_BV(IR_BIT_LEFT);		/* Pin as input */
	//IR_PORT &= ~_BV(IR_BIT_LEFT);		/* Enable internal pull-down */

	IR_DDR &= ~_BV(IR_BIT_RIGHT);		/* Pin as input */
	//IR_PORT &= ~_BV(IR_BIT_RIGHT);	/* Enable internal pull-down */
	
	IR_DDR &= ~_BV(IR_BIT_CENTER);		/* Pin as input */
	//IR_PORT &= ~_BV(IR_BIT_CENTER);	/* Enable internal pull-down */
	
	IR_DDR &= ~_BV(IR_BIT_REAR);		/* Pin as input */
	//IR_PORT &= ~_BV(IR_BIT_REAR);	    /* Enable internal pull-down */
}

void pwm_init(void)
{
	/* Set OC1A/OC1B on Compare Match when up-counting.
	Clear OC1A/OC1B on Compare Match when down-counting. */
	TCCR1A |= _BV(COM1A1) | _BV(COM1B1); // | _BV(COM1A0) | _BV(COM1B0);
	/* Phase Correct and Phase and Frequency Correct PWM */
	TCCR1B |= _BV(WGM13);
	/* Pre-scalar 1 */
	TCCR1B |= _BV(CS10);
	/* set the TOP value ICR1 */
	ICR1 = TOP;
	/* Start with 50% PWM */
	OCR1A = BOTTOM;
	OCR1B = BOTTOM+300;
}

void timer0_init(void){
	// configure millisecond timing
	// F_CPU == 8000000, prescaler = 64, resolution = 125: 8000000/64/125 = 1000
	TCCR0 = _BV(WGM01) | _BV(CS01) | _BV(CS00);  // CTC mode, prescaler = 8
	OCR0 = 124;
	TIMSK |= _BV(TOIE0);
}

uint32_t milliseconds(void) {
	uint32_t timer0_milliseconds_local;
	ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
		timer0_milliseconds_local = timer0_milliseconds;
	}
	return timer0_milliseconds_local;
}

void booting(void)
{
	int i;
	lcd_puts("-- Booting... --\n");
	/*
    * load two user defined characters from program memory
    * into LCD controller CG RAM location 0 and 1
    */
	lcd_command(_BV(LCD_CGRAM));  /* set CG RAM start address 0 */
	for(i=0; i<16; i++)
	{
		lcd_data(pgm_read_byte_near(&blockChar[i]));
	}
	for(i=0; i<16; i++)
    {
		/* create illusion of progress bar */
		_delay_ms(LCDDELAY);
		/* increment cursor to position i on line 2 */
        lcd_gotoxy(i,1);  
        /* write single char to display */
        lcd_putc(0);
	}	
	
	lcd_clrscr();
	lcd_puts("Avoibot Running\n");
}

void left_motor_dir(uint8_t direction)
{
	switch (direction){
		case M_FORWARD:
		C_CLRBIT(M_IN1);
		C_SETBIT(M_IN2);
		break;
			
		case M_REVERSE:
		C_SETBIT(M_IN1);
		C_CLRBIT(M_IN2);
		break;
			
		case M_STALL:
		C_CLRBIT(M_IN1);
		C_CLRBIT(M_IN2);
		break;
			
		case M_STOP:
		C_SETBIT(M_IN1);
		C_SETBIT(M_IN2);
		break;
	}
}

void right_motor_dir(uint8_t direction)
{

	switch (direction){
		case M_FORWARD:
		C_CLRBIT(M_IN3);
		C_SETBIT(M_IN4);
		break;
			
		case M_REVERSE:
		C_SETBIT(M_IN3);
		C_CLRBIT(M_IN4);
		break;
			
		case M_STALL:
		C_CLRBIT(M_IN3);
		C_CLRBIT(M_IN4);
		break;
			
		case M_STOP:
		C_SETBIT(M_IN3);
		C_SETBIT(M_IN4);
		break;
	}
}

void sensor_display(const char *Text)
{
	/*Resetting line 2*/
	lcd_gotoxy(0,0);
	lcd_puts(Text);
}

void steer_display(const char *Direction)
{
	/*Resetting line 2*/
	lcd_gotoxy(0,1);
	lcd_puts(Direction);
}

uint8_t sensor_read()
{
	uint8_t SENSOR=IR_PIN;
	SENSOR &= 0x0F;
	char string[16];
	utoa(SENSOR,string,16);
	strcat(string,"               ");
	sensor_display(string);
	return SENSOR;
}

void steer_robot(uint8_t direction)
{
	switch (direction)
	{
		case GO_RIGHT:
		left_motor_dir(M_FORWARD);
		right_motor_dir(M_STOP);
		steer_display("GO RIGHT         ");
		break;
		
		case GO_HARD_RIGHT:
		left_motor_dir(M_FORWARD);
		right_motor_dir(M_REVERSE);
		steer_display("GO HARD RIGHT    ");
		break;
		
		case GO_FORWARD:
		left_motor_dir(M_FORWARD);
		right_motor_dir(M_FORWARD);
		steer_display("GO FORWARD       ");
		break;
		
		case GO_REVERSE:
		left_motor_dir(M_REVERSE);
		right_motor_dir(M_REVERSE);
		steer_display("GO REVERSE       ");
		break;
		
		case GO_HARD_LEFT:
		left_motor_dir(M_REVERSE);
		right_motor_dir(M_FORWARD);
		steer_display("GO HARD LEFT     ");
		break;
		
		case GO_LEFT:
		left_motor_dir(M_STOP);
		right_motor_dir(M_FORWARD);
		steer_display("GO LEFT          ");
		break;
		
		case GO_BRAKE:
		left_motor_dir(M_STOP);
		right_motor_dir(M_STOP);
		steer_display("GO BRAKE         ");
		break;
	}
}

int main(void)
{
	/* input/output initializations */
	io_init();

	/* pwm setup for motor */
	pwm_init();

	/* timer0 setup for time tracking */
	timer0_init();
	
	/* other inits */
	init();
	
	/* initialize LCD display, cursor off */
	lcd_init(LCD_DISP_ON);
	lcd_clrscr();   /* clear display home cursor */

	/* Booting animation */
	booting();
	
	/* variable initializations */
	static uint32_t obstacle_last_seen_at;
	uint8_t sensor;
	
	/* Main loop */
	while(1){
		// detection
		sensor=sensor_read();
		if (sensor==1 || sensor==2 || sensor == 4 || sensor == 8) obstacle_last_seen_at = milliseconds();

		// action
		if ((milliseconds() - obstacle_last_seen_at) < REVERSE_DELAY) {
			steer_robot(GO_REVERSE);
		}
		else if((milliseconds() - obstacle_last_seen_at) < NEW_COURSE_DELAY) {

			switch(sensor){
				case S_OFF:
				steer_robot(GO_FORWARD);
				break;
				case S_LEFT:
				steer_robot(GO_HARD_RIGHT);
				break;
				case S_RIGHT:
				steer_robot(GO_HARD_LEFT);
				break;
				case S_CENTER:
				steer_robot(GO_REVERSE);
				break;
				case S_REAR:
				steer_robot(GO_BRAKE);
				break;
				case S_LEFT_RIGHT:
				steer_robot(GO_REVERSE);
				break;
				case S_LEFT_CENTER:
				steer_robot(GO_HARD_RIGHT);
				break;
				case S_CENTER_RIGHT:
				steer_robot(GO_HARD_LEFT);
				break;
				case S_LEFT_CENTER_RIGHT:
				steer_robot(GO_HARD_RIGHT);
				break;
				case S_LEFT_REAR:
				steer_robot(GO_HARD_RIGHT);
				break;
				case S_RIGHT_REAR:
				steer_robot(GO_HARD_LEFT);
				break;
				case S_LEFT_RIGHT_REAR:
				steer_robot(GO_HARD_LEFT);
				break;
				case S_CENTER_REAR:
				steer_robot(GO_HARD_RIGHT);
				break;
				case S_LEFT_CENTER_REAR:
				steer_robot(GO_HARD_RIGHT);
				break;
				case S_CENTER_RIGHT_REAR:
				steer_robot(GO_HARD_LEFT);
				break;
				case S_LEFT_CENTER_RIGHT_REAR:
				steer_robot(GO_HARD_LEFT);
				break;
			}		
		}
		else {
			steer_robot(M_FORWARD);
		}
	}
	return 0;
}

